#pragma once
#include "m3508.h"
#include "gm6020.h"
#include "m2006.h"
#include "RoboMaster.h"
#include "capacitor.h"
#include "kalman.h"
#include "ewma.h"
#include "pid.h"

// The encoder value corresponding to mechanical zero of the gimbal motor
#define GIMBAL_YAW_ENCODER_ZERO_VALUE ( (int32_t) 2310 )
#define GIMBAL_PITCH_ENCODER_ZERO_VALUE ( (int32_t) 266 )
#define YAW_ENCODER_ZERO_VALUE ( (int32_t) 3391)//7473
// The encoder value corresponding to mechanical zero of the chassis motor
#define CHASSIS_FRA_ENCODER_ZERO_VALUE ( (int32_t) 0)
#define CHASSIS_FLA_ENCODER_ZERO_VALUE ( (int32_t) 0)

#define CHASSIS_FRL_ENCODER_ZERO_VALUE ( (int32_t) 0 )
#define CHASSIS_FLL_ENCODER_ZERO_VALUE ( (int32_t) 0 )
#define CHASSIS_BRL_ENCODER_ZERO_VALUE ( (int32_t) 0 )
#define CHASSIS_BLL_ENCODER_ZERO_VALUE ( (int32_t) 0 )

// The resolution of encoder
#define ENCODER_RESOLUTION ( (uint32_t) 8192)

// The reduction ratio of chassis motor
#define CHASSIS_STEER_MOTOR_REDUCTION_RATIO ( (uint32_t) 1 )
#define CHASSIS_SPEED_MOTOR_REDUCTION_RATIO ( (uint32_t) 19 )

// The reduction ratio of gimbal motor
#define GIMBAL_MOTOR_REDUCTION_RATIO ( (uint32_t) 1 )

// The reduction ratio of shoot motor
#define SHOOT_MOTOR_REDUCTION_RATIO ( (uint32_t) 36 )

// The reduction ratio of fric motor
#define FRIC_MOTOR_REDUCTION_RATIO ( (uint32_t) 1 )

// Motor ID(same type motors should have different ID)
#define GIMBAL_YAW_MOTOR_ID ( (uint8_t) 2 )
#define GIMBAL_PITCH_MOTOR_ID ( (uint8_t) 1 )

#define YAW_MOTOR_ID ( (uint8_t) 3 )

#define CHASSIS_LINE_FL_MOTOR_ID    ((uint8_t) 4 )
#define CHASSIS_LINE_FR_MOTOR_ID    ((uint8_t) 3 )
#define CHASSIS_LINE_BR_MOTOR_ID    ((uint8_t) 2 )
#define CHASSIS_LINE_BL_MOTOR_ID    ((uint8_t) 1 )

// #define CHASSIS_STEER_BR_MOTOR_ID    ((uint8_t) 1 )
// #define CHASSIS_STEER_BL_MOTOR_ID    ((uint8_t) 2 )

#define UP_FRIC_MOTOR_ID                      ( (uint8_t) 4 )
#define DOWN_FRIC_MOTOR_ID                     ( (uint8_t) 2 )
#define SUPPLY_BULLET_MOTOR_ID           ( (uint8_t) 1 )

// Motor number
#define CHASSIS_STEER_MOTOR_NUM ( (uint8_t) 2)
#define CHASSIS_LINE_MOTOR_NUM ( (uint8_t) 4 )
#define GIMBAL_MOTOR_NUM ( (uint8_t) 3 )
#define SUPPLY_BULLET_MOTOR_NUM ( (uint8_t) 1 )
#define FRICTION_MOTOR_NUM ( (uint8_t) 2 )

// Robot mannal control sensitivity
#define GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY       ( (float) 0.05f )
#define GIMBAL_YAW_ANGLE_MANNAL_CONTROL_SENSITIVITY         ( (float) 0.05f )
#define GIMBAL_PITCH_ANGLE_SCAN_SPEED                       ( (float) 0.1f )
#define GIMBAL_YAW_ANGLE_SCAN_SPEED                         ( (float) 0.15f )
#define YAW_ANGLE_SCAN_SPEED                                ( (float) 0.075f )                     
#define GIMBAL_PITCH_ANGLE_SCAN_ANGLE_MIN                   ( (float) -10.0f )
#define GIMBAL_PITCH_ANGLE_SCAN_ANGLE_MAX                   ( (float) 10.0f )
#define GIMBAL_YAW_ANGLE_SCAN_ANGLE_MIN                     ( (float) -70.0f )
#define GIMBAL_YAW_ANGLE_SCAN_ANGLE_MAX                     ( (float) 70.0f )
#define GIMBAL_PITCH_MAX                                    ( (float) 30.0f )
#define GIMBAL_PITCH_MIN                                    ( (float) -20.0f )
#define GIMBAL_YAW_MAX                                      ( (float) 70.0f )
#define GIMBAL_YAW_MIN                                      ( (float) -70.0f )


#define CHASSIS_MANNAL_LINE_SPEED_MAX                              ( (float)(3.5) )
#define CHASSIS_MANNAL_SPIN_SPEED_MAX                                ( (float)(3.5) )
#define CHASSIS_SPIN_OVER_LIMIT                             ( (float)(0.05) )
#define CHASSIS_LINE_SPEED_MAX                              ( (float)(4.5) )
#define CHASSIS_SPIN_SPEED_MAX                              ( (float)(6) )
#define CHASSIS_HEAD_FOLLOW_Kp                              ( (float)(0.008))
#define CHASSIS_HEAD_FOLLOW_Kd                              ( (float)(0.001))

#define CHASSIS_TIGER_AMP                                   ( (float)(90))
#define CHASSIS_TIGER_T                                     ( (float)(2))

#define CHASSIS_WHEEL_RADIUS                                (float)(0.07f)
    
#define RIDAR_FIXED_ANGLE_ERROR                            (float)(0.0f)

#define SPIN_ANGLE                                      (float)(45.0f)//待修改

#define SHOOT_BULLET_FREQUENCY_TEST_0   0
#define SHOOT_BULLET_FREQUENCY_TEST_1   1
#define SHOOT_BULLET_FREQUENCY_TEST_2   6
#define SHOOT_BULLET_FREQUENCY_TEST_3   13
#define SHOOT_BULLET_FREQUENCY_TEST_4   20



#define SHOOT_DRIVER_SUPPLY_ANGLE_STEP  ((float)(360/8))

#define SHOOT_SPEED_FILTER_BETA ((float)0.9)//弹速反馈滤波

#define GIMBAL_YAW_IMU_FEED_BACK                             
// #define GIMBAL_YAW_ENCODER_FEED_BACK                         

#define G_COMPENSATION ((float)1500.0f)//pitch前馈补偿系数
#define FRIC_WHEEL_SPEED                ((float)4500.0f)//TODO: 【发射】修改摩擦轮速度
#define FRIC_WHEEL_SPEED_SAFE           ((float)3500.0f)


#define FORWARD_DAMP_FACTOR                                     ((float)(800))//驱动轮前馈
#define WHEEL_CURRENT_DEAD_AREA                                   ((float)(1000))

#define Pitch_Vision_Error                          ((float) 1.0f)
#define Yaw_Vision_Error                           ((float) 5.0f)

#define PRESS_Q                                       81
#define PRESS_W                                       87
#define PRESS_E                                       69
#define PRESS_C                                       67
#define PRESS_F                                       70
#define PRESS_G                                       71
#define PRESS_R                                       82
#define PRESS_V                                       86                                      
#define PRESS_X                                       88
#define PRESS_Z                                       90


/***************************键盘信息***************************
*域              按键                                        *
*偏移            102                                         *
*长度            16                                          *
*符号位          无                                          *
*范围            位值标识                                     *
*功能         每个按键对应一个bit                              *
*                Bit0:W键                                    *
*                Bit1:S键                                    *
*                Bit2:A键                                    *
*                Bit3:D键                                    *
*                Bit4:Shift键                                *
*                Bit5:Ctrl键                                 *
*                Bit6:Q键                                    *
*                Bit7:E键                                    *
**************************************************************/

#define KEY_PRESSED_OFFSET_W        ((u16)0x01<<0)
#define KEY_PRESSED_OFFSET_S        ((u16)0x01<<1)
#define KEY_PRESSED_OFFSET_A        ((u16)0x01<<2)
#define KEY_PRESSED_OFFSET_D        ((u16)0x01<<3)
#define KEY_PRESSED_OFFSET_SHIFT    ((u16)0x01<<4)
#define KEY_PRESSED_OFFSET_CTRL     ((u16)0x01<<5)
#define KEY_PRESSED_OFFSET_Q        ((u16)0x01<<6)
#define KEY_PRESSED_OFFSET_E        ((u16)0x01<<7)
#define KEY_PRESSED_OFFSET_R        ((u16)0x01<<8)
#define KEY_PRESSED_OFFSET_F        ((u16)0x01<<9)
#define KEY_PRESSED_OFFSET_G        ((u16)0x01<<10)
#define KEY_PRESSED_OFFSET_Z        ((u16)0x01<<11)
#define KEY_PRESSED_OFFSET_X        ((u16)0x01<<12)
#define KEY_PRESSED_OFFSET_C        ((u16)0x01<<13)
#define KEY_PRESSED_OFFSET_V        ((u16)0x01<<14)
#define KEY_PRESSED_OFFSET_B        ((u16)0x01<<15)

//TODO: 【键鼠】减速加速速率
#define LineAccelStep                                             15   //加速斜率
#define LineDecelStep                                             15//50    //减速斜率

//TODO:【底盘】修改底盘参数
// chassis parameters
#define WHEEL_L2R_DIST      ((float) 304.44)
#define WHEEL_F2B_DIST      ((float) 354)
#define WHEEL2CENTER_DIST   ((float) 233.452197)
#define WHEEL_ANGLE         ((float) 30.0f)
#define WHEEL2CENTER_ANGLE  ((float) 40.695531)
#define WHEEL_RADIUS        ((float) 0.06)

#define AIM_ASSIST_TEST_PITCH_MIN                           ( (float) -5.0f )
#define AIM_ASSIST_TEST_PITCH_MAX                           ( (float) 5.0f )
#define AIM_ASSIST_TEST_YAW_MIN                             ( (float) -8.0f )
#define AIM_ASSIST_TEST_YAW_MAX                             ( (float) 0.0f )

#define AIM_ASSIST_TEST_SPEED                               ( (float) 0.06f )

//TODO:[裁判系统-打弹热量控制]
#define ShooterHeat_mes     ((s32)G_referee.PowerHeatData.shooter_id1_17mm_cooling_heat)    //枪口热量
#define ShooterHeat_Coll    ((float)G_referee.GameRobotStatus.shooter_barrel_cooling_value)//枪口热量冷却
#define ShooterHeat_Limit   ((s32)G_referee.GameRobotStatus.shooter_barrel_heat_limit) //枪口热量上限
#define ShooterSpeed_Limit  25    //枪口射速上限
#define PelletSpeed_mes     ((float)G_referee.ShootData.bullet_speed)   

// //param
// #define PITCH_STEP_TD_K                                             ( (float) 0.5 )
// #define PITCH_STEP_PID_KP                                           ( (float) 40 )
// #define PITCH_STEP_PID_KI                                           ( (float) 0 )
// #define PITCH_TRACK_TD_K                                            ( (float) 1 )
// #define PITCH_TRACK_PID_KP                                          ( (float) 70 )
// #define PITCH_TRACK_PID_KI                                          ( (float) 0.15 )
// #define PITCH_STEP_THRESHOLD                                        ( (float) 4.0f )

// #define YAW_STEP_TD_K                                               ( (float) 0.6 )
// #define YAW_STEP_PID_KP                                             ( (float) 25 )
// #define YAW_TRACK_TD_K                                              ( (float) 0.8 )
// #define YAW_TRACK_PID_KP                                            ( (float) 40 )
// #define YAW_STEP_THRESHOLD                                          ( (float) 4.0f )


class SentryRobot {
public:
    enum ChassisMode {
        CHASSIS_SAFE = 0,//安全模式，无电流输出
        CHASSIS_MANNAL,//手动模式，遥控器操作
        CHASSIS_AUTO,//自动模式
        CHASSIS_TEST,
        CHASSIS_KEYMOUSE //键鼠模式
    };
    ChassisMode chassis_mode;
    ChassisMode chassis_mode_pre;

    enum GimbalMode {
        GIMBAL_SAFE = 0,//安全模式，无电流输出
        GIMBAL_MANNAL,//手动模式，遥控器操作
        GIMBAL_AUTO,//自动模式
        GIMBAL_KEYMOUSE //键鼠模式
    };
    GimbalMode gimbal_mode;
    GimbalMode gimbal_mode_pre;

    enum ShootMode {
        SHOOT_SAFE = 0,//安全模式，无电流输出
        SHOOT_MANNAL,//手动模式，遥控器操作
        SHOOT_AUTO,//自动模式
        SHOOT_HALFAUTO,//半自动模式?
        SHOOT_KEYMOUSE//键鼠模式
    };
    ShootMode shoot_mode;
    ShootMode shoot_mode_pre;

    enum ChassisLineMotor 
    {
        CHASSIS_L_FL_MOTOR = 0,
        CHASSIS_L_FR_MOTOR,
        CHASSIS_L_BL_MOTOR,
        CHASSIS_L_BR_MOTOR
    };
    enum ChassisSteerMotor
    {
        CHASSIS_S_BL_MOTOR = 0,
        CHASSIS_S_BR_MOTOR
    };
    M3508* chassis_line_motor[CHASSIS_LINE_MOTOR_NUM];
    GM6020* chassis_steer_motor[CHASSIS_STEER_MOTOR_NUM];

    enum GimbalMotor {
        GIMBAL_PITCH_MOTOR = 0,
        GIMBAL_YAW_MOTOR,
        YAW_MOTOR
    };
    GM6020* gimbal_motor[GIMBAL_MOTOR_NUM];

    enum SupplyBulletMotor {
        SHOOT_DRIVE_MOTOR = 0
    };
    enum FrictionMotor {
        UP_FRICTION_MOTOR = 0,
        DOWN_FRICTION_MOTOR
    };
    M2006* supply_bullet_motor[SUPPLY_BULLET_MOTOR_NUM];
    M3508* friction_motor[FRICTION_MOTOR_NUM];

    Capacitor* capacitor;


    // chassis value
    enum ChassisAngleMode
    {
        SpeedTrackFron = 0,//单向模式，永远头朝前，云台控制方向 、 底盘跟随云台
        SpeedTrackDoub,//双向模式，前后等效
        Tiger,//虎式模式，叠加正弦步伐
        Gyro,//小陀螺
        Follow,//底盘跟随云台
        KEYMOUSE_FOLLOW,//键鼠模式,底盘跟随键鼠
        KEYMOUSE_SAFE//键鼠模式,底盘不动（在世界坐标系中维持当前位置）      
    };
    ChassisAngleMode chassis_w_mode;
    uint8_t yaw_control_flag;//等于0触发 TODO:【底盘】?yaw控制模式 底盘是否跟随云台
    Pid *m_chassis_angle_pid;
    uint8_t chassis_angle_test_flag;
    float chassis_angle_test_target;
    float number_test1;
    float number_test2;

    float m_chassis2world_angle_current;//车底盘角度当前值 逆时针正向
    float m_chassis2world_angle_target;//车底盘角度目标值
    float m_chassis_speed_target;//目标速度
    float m_chassis_spin_speed_target;//车目标转速
    float m_chassis_speed2radar_angle_target;//目标速度方向，雷达坐标系
    float m_chassis_speed2chassis_angle_target;//目标速度方向，相对底盘
    float m_chassis_speed;//车实际速度大小
    float m_chassis_speed2world_angle;//车实际速度方向 世界坐标系
    float m_chassis_speed2chassis_angle;//车实际速度方向 底盘坐标系
    float m_chassis_spin_speed;//车实际转速
    
    float m_robot_radar_world_yaw;//雷达在世界系下的角度，增量式表示.
    float m_robot_radar_world_yaw_abs;//雷达在世界系下的角度，绝对式表示
    float m_robot_radar_world_yaw_cor_abs;////雷达在世界系下的角度，绝对式表示,添加角速度修正
    float m_robot_radar_world_yaw_filter;//雷达在世界系下的角度，滤波器处理后

    float m_steer_wheel_gain_k;
    float m_chassislimit_pow_k; //底盘功率控制系数
    float m_chassislimit_speedtrack_k;
    float m_chassislimit_response_k;
    
    //键鼠模式相关变量
    float  DirectX_Coe = 1.0f;  //键盘鼠标模式下，x轴系数，左右方向
    float  DirectY_Coe = 1.0f;  //键盘鼠标模式下，y轴系数，前后方向
    //TODO:[键鼠]键鼠模式下速度限制  疑问：是否要命名规范，此处过于简单
    float x_speed_max = 30.0; //键鼠模式x轴速度限制
    float y_speed_max = 30.0; //键鼠模式y轴速度限制

    typedef struct {
        s16 X; //x坐标
        s16 Y; //y坐标
        s16 Z; //z坐标
        uint8_t Left;    //左键状态（1：按下；0：没按下）
        uint8_t Right;   //右键状态（1：按下；0：没按下）
    } ST_MOUSE; //鼠标结构体
    ST_MOUSE stMouse; //鼠标结构体
    float KeyMouse_SST_Yaw   = 0.00102f;    //键鼠模式 先暂时为两个YAW轴灵敏度
    float KeyMouse_SST_Pitch = 0.00035f;  //键鼠模式 PITCH轴灵敏度

    float KeyMouse_Point_SIN; //键鼠模式下，鼠标指向角度的sin值
    float KeyMouse_Point_COS; //键鼠模式下，鼠标指向角度的cos值

    float WAccelStep = 180.0f; //小陀螺加速斜率0.015/s2
    float Top_Speed_Keyboard = 3000.0f; //键鼠模式下小陀螺最大速度（立方）

    float keymouse_gimbal_yaw_error = 0; //键鼠模式下，小yaw是否到达误差范围

    //TODO:[键鼠+发射]键鼠模式下的发射参数(此处采用结构体)
    typedef struct  
   {
       s32 Key_Left_Time_Cnt;//对鼠标单次点击是否有效的计时器(两次点击之间间隔180ms)
       s32 Key_Lefting_Time_Cnt;//对鼠标长按的计时器(长按280ms进入连发模式)
        s32 Key_Righting_Time_Cnt;//对鼠标右键长按的计时器(长按280ms进入辅瞄模式)
        bool Key_Is_Shoot;//判断是否可以发射的标志位
        bool Key_Is_Assit;//判断是否进入辅瞄的标志位
    } Struct_Shoot_KEYMOUSE;
    
    Struct_Shoot_KEYMOUSE Shoot_KEYMOUSE;

    // 射击模式枚举
    enum Enum_Shoot_Turn_Mode 
    {     
    SingleShoot = 0,     // 单发模式（每次点击发射一发）
    RunningShoot    // 连发模式（自动持续射击）
    };
    Enum_Shoot_Turn_Mode Shoot_Turn_Mode ; //默认单发模式

    //TODO:[键鼠]云台旋转180
    #define ROTATION_STEP 10 //每次旋转步长
    #define TOTAL_ROTATION_COUNT 18//总旋转次数
    typedef enum 
    {
        ROTATION_IDLE = 0,       // 空闲状态
        ROTATION_IN_PROGRESS, // 旋转进行中
        ROTATION_WAITING      // 等待状态
    } RotationState;
    
    RotationState rotation_state;//默认空闲状态
    int rotation_count = 0;//旋转次数计数器
    unsigned long last_rotation_time = 0;
    const unsigned long rotation_delay = 30000; // 每次旋转后的等待时间50ms（单位：毫秒）
    unsigned long gimbal_turn_current_time = 0;

    int gyro_control_flag = 0; //陀螺控制标志位

    //TODO:[打弹热量控制]结构体
    typedef struct {
        float Heat_Left ;//剩余热量 即当前热量与上限差值
        float shoot_num ;//弹丸数量
        float Pre_ShootSpeed ;//记录上一次弹丸速度
        s16 Allowed_PelletNum ;  //一个周期内（100ms）实时可发射弹丸数量,受热量限制
        u8 PelletNum ;            //已经发射多少颗子弹，每次重载子弹自动清零
        float ShooterPosDes_Pre ; //记录上一次拨盘目标位置
        } Struct_Shoot_HEAT;
        Struct_Shoot_HEAT Shoot_Heat_Control;

        s32 Heatlimit = 0; //热量限制

/*UI绘图相关结构体及变量定义*/
#pragma pack(push)
#pragma pack(1)
    //UI绘图
    u8 UI_New_Client_Cnt = 1;
    u8 UI_ClientCommand = 1;
    u8 UI_Start_Cnt = 30;
    u8 UI_Static_Char_Cnt =1;
    u8 UI_RefreshCnt = 0;
   //交互数据接收信息： 0x0301。
typedef  struct
{
    uint16_t data_cmd_id;
    uint16_t sender_ID;
    uint16_t receiver_ID;
} ext_student_interactive_header_data_t;

//1.交互数据 机器人间通信： 0x0301。
typedef  struct
{
    uint8_t m_ReceiveData[113];
    uint8_t m_SendData[113];
} robot_interactive_data_t;

//2.客户端删除图形 机器人间通信： 0x0301。
typedef  struct
{
    uint8_t operate_tpye;
    uint8_t layer;
} ext_client_custom_graphic_delete_t;

//图形数据
typedef  struct
{
    uint8_t graphic_name[3];
    uint32_t operate_tpye:3;
    uint32_t graphic_tpye:3;
    uint32_t layer:4;
    uint32_t color:4;
    uint32_t start_angle:9;
    uint32_t end_angle:9;
    uint32_t width:10;
    uint32_t start_x:11;
    uint32_t start_y:11;
    uint32_t radius:10;
    uint32_t end_x:11;
    uint32_t end_y:11;
} graphic_data_struct_t;

//3.客户端绘制一个图形 机器人间通信： 0x0301。
typedef  struct
{
    graphic_data_struct_t grapic_data_struct;
} ext_client_custom_graphic_single_t;

//4.客户端绘制二个图形 机器人间通信： 0x0301。
typedef  struct
{
    graphic_data_struct_t grapic_data_struct[2];
} ext_client_custom_graphic_double_t;

//5.客户端绘制五个图形 机器人间通信： 0x0301。
typedef  struct
{
    graphic_data_struct_t grapic_data_struct[5];
} ext_client_custom_graphic_five_t;

//6.客户端绘制七个图形 机器人间通信： 0x0301。

typedef   struct
{
    graphic_data_struct_t grapic_data_struct[7];
} ext_client_custom_graphic_seven_t;

//7.客户端绘制字符 机器人间通信： 0x0301。
typedef  struct
{
    graphic_data_struct_t grapic_data_struct;
    uint8_t data[30];
} ext_client_custom_character_t;

typedef enum
{
	en_MyColor  = 0,
  	en_Yellow   = 1,
	en_Green	= 2,
	en_Orange	= 3,
	en_Purple	= 4,
	en_Pink		= 5,
  	en_Grblue   = 6,
  	en_Black 	= 7,
	en_White 	= 8,
} EN_Client_PictureColor;

typedef struct
{
	EN_Client_PictureColor Cap_Color;				//电容图层颜色
  	float Cap_Energy;								//电容剩余能量
	float Cap_Length;								//电容图层长度
	float Volume_Y0;								//体积碰撞纵向基准初值
	float Volume_X0;								//体积碰撞横向基准初值
	float Volume_Y1;								//体积碰撞纵向基准末值
	float Volume_X1;								//体积碰撞横向基准末值	
	float Volume_Armor;							    //装甲板基准
	float Volume_Y;									//体积碰撞纵向基准
	float Volume_X;									//体积碰撞横向基准	
	float Volume_Angle_Inc;					        //体积碰撞插值斜率
	float Volume_K;									//体积碰撞斜边斜率
	int Bullet_Num;
} ST_Client_Sendata_Coe;

typedef struct
{
	float X1;		
	float Y1;		
	float X2;			
	float Y2;		
	float X3;		
	float Y3;
	float X4;		
	float Y4;
	float X5;		
	float Y5;
	float X6;		
	float Y6;
} ST_Client_Draw_Ruler;

typedef struct
{
	float X1;		
	float Y1;		
	float X2;		
	float Y2;
} ST_Client_Assist_Aim;

typedef struct
{	
	float X_Nearest;
	float Y_Nearest;
	float Pitch_Nearest;
	
	float X_Near;
	float Y_Near;
	float Pitch_Near;
	
	float X_Middle;
	float Y_Middle;
	float Pitch_Middle;
	
	float X_Far;
	float Y_Far;
	float Pitch_Far;
	
	float X_Farest;
	float Y_Farest;
	float Pitch_Farest;
	
	float X_High_Middle;
	float Y_High_Middle;
	float Pitch_High_Middle;
	
	float X_High_Near;
	float Y_High_Near;
	float Pitch_High_Near;
	
	float X;		
	float Y;	
} ST_Client_Movable_Front_Sight;

typedef struct
{
	u8 CAP_Energy;
	u8 Pellet_Num;
} ST_Data_to_Ariel;

typedef struct
{
	u8 CAP_Energy;
	u8 Pellet_Num;
	u8 Soldier_X;
	u8 Soldier_Y;
	u8 Soldier_Z;
} ST_Data_to_Radar;
typedef unsigned char *byte_pointer;

typedef struct
{
    float X[7];           //自变量特征点
    float Y[7];           //因变量特征点
	float a[7];      	  //过程结果
	float Des;     		  //输入
	float Output; 		  //输出
} ST_Lagrange;

typedef struct
{
    int X_Distance ;	//X轴方向距离
	int Y_Distance ;	//Y轴方向距离
	int Cap_Timer ;
	int CAP_Percent ;
} ST_UI_Send_Data;

#pragma pack(pop)

 ext_student_interactive_header_data_t	    G_ST_Student_Interactive_Header_Data;			//交互数据接收信息
 robot_interactive_data_t					G_ST_Robot_Interactive_Data;				    //1.交互数据
 ext_client_custom_graphic_delete_t			G_ST_Client_Custom_Graphic_Delete;				//2.客户端删除图形
 ext_client_custom_graphic_single_t			G_ST_Client_Custom_Graphic_Single;				//3.客户端绘制一个图形
 ext_client_custom_graphic_double_t			G_ST_Client_Custom_Graphic_Double;				//4.客户端绘制二个图形
 ext_client_custom_graphic_five_t		  	G_ST_Client_Custom_Graphic_Five;				//5.客户端绘制五个图形
 ext_client_custom_graphic_seven_t			G_ST_Client_Custom_Graphic_Seven;				//6.客户端绘制七个图形
 ext_client_custom_character_t			    G_ST_Client_Custom_Character;					//7.客户端绘制字符

 ST_Client_Movable_Front_Sight Client_Movable_Front_Sight;
 ST_Client_Movable_Front_Sight Client_Movable_Front_Sight_15;
 ST_Client_Movable_Front_Sight Client_Movable_Front_Sight_18;
 ST_Client_Movable_Front_Sight Client_Movable_Front_Sight_30;
 float Client_Armor_Normal_0;
 float Client_Armor_Normal_24;

 ST_Client_Assist_Aim Client_Aisisted_Aim_Ruler_0;
 ST_Client_Assist_Aim Client_Aisisted_Aim_Ruler_15;
 ST_Client_Draw_Ruler Client_Draw_Ruler;

 float Client_VolumeY_Normal_0;
 float Client_VolumeX_Normal_0;
 float Client_VolumeY_Normal_24;
 float Client_VolumeX_Normal_24;
 float Client_VolumeX_Koffset_Normal;			//体积碰撞图像梯形上下边的斜率

 float Client_VolumeY_Top_0;
 float Client_VolumeX_Top_0;
 float Client_VolumeY_Top_20;
 float Client_VolumeX_Top_20;
 float Client_VolumeX_Koffset_Top;			//体积碰撞图像梯形上下边的斜率
 ST_Client_Sendata_Coe Client_Sendata_Coe;

/****裁判系统通讯****/
/*主通讯函数*/
void New_Client(void);
void pictureOperation(graphic_data_struct_t* pGraphicData, u8* graphicName, u32 operateTpye, u32 graphicTpye,
                      u32 layer, u32 color, u32 startAngle, u32 endAngle, u32 width, u32 startX, u32 startY,
                      u32 radius, u32 endX, u32 endY);

/*应用级通讯函数*/
void sendPictureToClient(void* picture, u16 pictureNum);
void Client_Prepare_Normal_Data(u8 Command);
void Client_Prepare_Char_Data(u8 Command);
void Client_Prepare_Char_DebugData(u8 Command);
void Client_Prepare_Static_Char_Data(u8 Command,u8 Num);
void Client_Prepare_Num_Data(u8 Command);
void Send_Data_To_Radar(void);
void Send_Data_To_Aerial(void);
void Client_Enemy_HP(u8 Command);

void Client_UIColor_Decision(u16 data_rx, u16 target_rx, u16 data_tx, u16 target_tx);
void Client_Prepare_Char_DebugData_1(u8 Command);
void Client_Prepare_Char_DebugData_2(u8 Command);

void Client_Get_Vision_Target(void);
void Client_Write_Vision_Status(u8 Command);
void Client_Draw_Vision_Status(u8 Command);
void Client_Draw_Seven_line(u8 Command);
void Client_Draw_Console(u8 Command);
void Client_Draw_Radar_Message(u8 Command);
void Client_Draw_AutomaticPatrols(void);
void Client_Draw_Aim(u8 Command);

/*裁判系统辅助函数*/
float Asisted_Aim_Axis(u8 XorY,float Assisted_Pitch_Angle);
float LagrangeInsert(ST_Lagrange* Lag,u8 Start_Num,u8 End_Num);



    Pid *m_chassis_resposns_opti_pid;
    // Pid *m_chassis_pow_pid;
    
    //TODO: 【云台】云台前馈模式
    // gimbal  value
    enum FeedForward
    {
        NoFF = 0, //没有前馈
        ControlFF,//控制的时候有前馈？
        AlwaysFF//一直有前馈
    };
    FeedForward ff_mode;
    int gimbal_pitch_ff_current;//总前馈值=重力前馈+摩擦力前馈
    int gimbal_yaw_ff_current;
    int yaw_ff_current;
    uint8_t gimbal_yaw_stepff_flag;//TODO：【云台】？云台yaw步进模式？阶跃前馈 没用

    bool enemy_find_flag;
    float gimbal_pitch_des; //目标pitch
    float gimbal_yaw_des;
    float yaw_des;
    float gimbal_pitch_min;
    float gimbal_pitch_max;
    float gimbal_yaw_min;
    float gimbal_yaw_max;
    uint16_t m_vision_delay_cnt;//丢失目标后先保持当前角度再巡逻
    bool direction_flag;//切换巡逻方向
    bool gimbal_pitch_scan_dir;
    bool gimbal_yaw_scan_dir;
    bool yaw_scan_dir;
    bool yaw_safe_flag;//为0时大yaw电机不输出电流
    bool patrol_flag;//巡逻标志位
    bool stop_flag;//能量上限测试标志位

    uint8_t gimbal_test_flag;//TODO：【云台】云台测试标志位
    float gimbal_yaw_test_des;//调试时云台小yaw目标角度，debug赋值
    float gimbal_pitch_test_des;//调试时云台pitch目标角度，debug赋值
    float yaw_test_des;//调试时大yaw目标角度，debug赋值

    float lidar_error;

    float gimbal_roll;


//TODO: 【发射】射击相关参数（射击次数、射击频率、射击模式、射击速度等）
    // shoot   value
    EWMA* m_shoot_speed_filter;//给辅瞄的滤波弹速
    uint32_t m_shoot_bullet_cnt_target;
    uint8_t m_shoot_bullet_fps;//目标射击频率 1or20
    uint32_t m_shoot_bullet_last_increase_time;
    float shoot_driver_angle_target;
    float shoot_driver_start_angle;
    float m_shoot_speed_pre;
    uint8_t shoot_cool_value;
    uint8_t friction_speed_down_flag;
    uint16_t shoot_count;//射击次数
    uint8_t shoot_ff_flag;
    uint16_t action_time;
    uint16_t shoot_ff_current_max;
    

    float demo_shoot_spe = 6700;//TODO: 【调试】测速
    bool shoot_insurance;//射击保险 false时可以射击 true时不能射击
    bool shoot_gimbal_flag;
    bool supply_bullut_reset_flag;//供弹电机复位标志位 貌似为1时不发射
    bool shoot_cool_flag = false;//枪管需要冷却标志位

    bool enemy_track_flag;//
    bool enemy_attackable_flag;

    // //调参
    // typedef struct 
    // {
    //     float angle_kp;
    //     float angle_ki;
    //     float td_k;
    //     float speed_kp;
    // }GimbalPidPara;
    // typedef struct 
    // {
    //     GimbalPidPara gimbalpitch_para;
    //     GimbalPidPara gimbalyaw_para;
    //     GimbalPidPara yaw_para;
    // }ParaBox;
    // ParaBox para_box;


    float test_box1;
    float test_box2;
    float test_box3;
    
		float fl_speed_des, fr_speed_des, bl_speed_des, br_speed_des,speedw_des;
		float speed_x_des , speed_y_des, speed_w_des;
        float yaw_direction_speed, yaw_vertical_speed;
        // float GFF_cos , g_k ;
    void Init(void);

    void ExecuteGimbalAlgorithm(void);
    void ExecuteChassisAlgorithm(void);
    void ExecuteShootAlgorithm(void);
    void SetChassisSpeedTarget(float l_fl_motor, float l_fr_motor, float l_bl_motor, float l_br_motor);
    void SetChassisAngleSpeedTarget(float s_bl_motor, float s_br_motor);
    void SetChassisAngleTarget(float s_bl_motor, float s_br_motor);
    void SetGimbalAngleTarget(float g_yaw, float g_pitch, float yaw);
    void SetShootDriverTarget(float angle);
    void UpdateGimbalPitchState(float angle, float speed);
    void UpdateYawState(float angle, float speed);
    void UpdateGimbalYawState(float angle, float speed);
    void SendControlCommand(void);
    void IncreaseShootBulletTarget(uint32_t time_now);
    inline void SetChassisMode(ChassisMode mode) {chassis_mode_pre = chassis_mode; chassis_mode = mode;};
    inline void SetGimbalMode(GimbalMode mode) {gimbal_mode_pre = gimbal_mode; gimbal_mode = mode;};
    inline void SetShootMode(ShootMode mode) {shoot_mode_pre = shoot_mode; shoot_mode = mode;};
    void Power_Control(void);
    void SetChassisAngleTarget(float angle_des);
    void RampSignal(float* p_Output, float DesValue, float Step);
    
    int GetShootFeedForward(void);
    void UpdateGimbalPitchFeedForward(void);
    void UpdateGimbalYawFeedForward(void);
    void UpdateYawFeedForward(void);

    void ChassisPowControl_SpeedSet(uint8_t enable);
    void ChassisSpeedtrackOptimize(uint8_t enable);
    void ChassisResponseOptimize(uint8_t enable);

    float GetRadarAngleSpinDevCorrect(void);

    bool  Is_Heat_Safe(void);

    // float PitchFilterUpdate(float angle_origin, float speed_origin);

    // void SetMotorPara(GM6020* p_motor, GimbalPidPara* p_para);
    
    SentryRobot(void);

    ~SentryRobot(){};

private:
    void InitAllActuators(void);
    void InitAllSensors(void);
    void InitFeedForward(void);
};
